#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <mavros_msgs/GlobalPositionTarget.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>

#include <geometry_msgs/TwistStamped.h>
 
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

// #include "quadrotor_msgs/PositionCommand.h"
// #include "off_board_test/gimbal.h"

enum FSM_STATE { INIT, CHECK_ID, LEADER, FOLLOWER};
std::string state_str[4] = { "INIT",
                        "CHECK_ID",
                        "LEADER",
                        "FOLLOWER"};
FSM_STATE exec_state;


void gps_position_pid_control(sensor_msgs::NavSatFix set_gps_pos, sensor_msgs::NavSatFix current_gps_pos, float velocity_limit, float target_yaw);
geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat);
geometry_msgs::Point limit_velocity(float vx, float vy,float maximum);
Eigen::Vector3d drone_force_vector(sensor_msgs::NavSatFix gps1, sensor_msgs::NavSatFix gps2);
void changeFSMExecState(FSM_STATE new_state, std::string pos_call);
void printFSMExecState();
void vel_command_send(geometry_msgs::Point vel, double yaw_rate);


mavros_msgs::State current_state;
ros::Subscriber altitude_sub;
ros::Subscriber rcin_sub;
ros::Subscriber state_sub;
ros::Subscriber odometrysub;
ros::Subscriber velocitysub;
ros::Subscriber gpssub;

ros::Subscriber master_sub, drone2_sub, drone3_sub, drone4_sub, drone5_sub;
ros::Publisher  local_vel_pub;

ros::ServiceClient arming_client;
ros::ServiceClient land_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient set_vechile_ned;

ros::Timer px4_control_timer;

geometry_msgs::Point current_point;
geometry_msgs::Point current_vel;
geometry_msgs::Quaternion current_angle;

geometry_msgs::Vector3 curr_angle;

double xyz_vel = 0.0;

float error_yaw_last = 0.0, error_yaw_integrated = 0.0;

double time_master_update = 0;

int print_state_cnt=0;
bool local_odom_flag = false;
float current_yaw_angle;
bool master_gps_update_flag = false;
bool drone2_update_flag = false;
bool drone3_update_flag = false;
bool drone4_update_flag = false;
bool drone5_update_flag = false;

geometry_msgs::Point error_pos_last, error_vel_last, error_pos_integrated, error_vel_integrated, attitude_expect, velocity_expected;


template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

double master_angle_yaw;
sensor_msgs::NavSatFix self_gps_pos;
sensor_msgs::NavSatFix master_gps_pos;

sensor_msgs::NavSatFix drone2_gps_pos;
sensor_msgs::NavSatFix drone3_gps_pos;
sensor_msgs::NavSatFix drone4_gps_pos;
sensor_msgs::NavSatFix drone5_gps_pos;

Eigen::Vector3d vector_1_2;
Eigen::Vector3d vector_1_3;
Eigen::Vector3d vector_1_4;
Eigen::Vector3d vector_1_5;
Eigen::Vector3d vector_2_1;
Eigen::Vector3d vector_2_3;
Eigen::Vector3d vector_2_4;
Eigen::Vector3d vector_2_5;
Eigen::Vector3d vector_3_1;
Eigen::Vector3d vector_3_2;
Eigen::Vector3d vector_3_4;
Eigen::Vector3d vector_3_5;
Eigen::Vector3d vector_4_1;
Eigen::Vector3d vector_4_2;
Eigen::Vector3d vector_4_3;
Eigen::Vector3d vector_4_5;
Eigen::Vector3d vector_5_1;
Eigen::Vector3d vector_5_2;
Eigen::Vector3d vector_5_3;
Eigen::Vector3d vector_5_4;

double dist_1_2 = 10;
double dist_1_3 = 10;
double dist_1_4 = 10;
double dist_1_5 = 10;
double dist_2_3 = 10;
double dist_2_4 = 10;
double dist_2_5 = 10;
double dist_3_4 = 10;
double dist_3_5 = 10;
double dist_4_5 = 10;

float master_vx, master_vy;
int formation_mode = 0;
double current_point_z = 0.0;
double juedui_z = 0.0;

int vel_command_cnt = 0;